#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include <string.h>

using namespace roboard_drivers;
ros::Subscriber	enc_sub;
ros::Publisher	mot_pub;
ros::Publisher	int_pub;

//Callback function for the "/encoder" topic.
void recive_encoder(const Encoder::ConstPtr &msg)
{
	int right = msg->right;
	int left = msg->left;
	double timestamp = msg->timestamp;
	printf("%f:got encoder L:%i , false R:%i\n",timestamp,left,right);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "motorTest");
	ros::NodeHandle n;
	enc_sub = n.subscribe("/serial/encoder", 1000, recive_encoder);

	mot_pub = n.advertise<Motor>("/serial/motor_speed", 100000);
	int_pub = n.advertise<std_msgs::Int32>("/serial/encoder_interval", 100000);
	ros::Rate loop_rate(10);
	struct timeval t;
	gettimeofday(&t, NULL);
	while(ros::ok()){
		gettimeofday(&t, NULL);
		Motor motor;
		double time = t.tv_sec+double(t.tv_usec)/1000000.0;
		motor.right	= sin(3.14*time/5.0);
		motor.left	= sin(3.14*time/5.0);
		mot_pub.publish(motor);
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}
